//
// Created by root on 22-11-2.
//

#include "Estimator.h"


Estimator::Estimator() {
    clearImuAndFeature();
}

void Estimator::clearImuAndFeature(){
    queue<sensor_msgs::ImuConstPtr> empty_imu;
    queue<sensor_msgs::PointCloudConstPtr> empty_fea;
    swap(imu_buf,empty_imu);
    swap(fea_buf,empty_fea);
    cout<<"initialize imu&image buf"<<endl;
}


void Estimator::process(){
   while(true){
        vector<pair<vector<sensor_msgs::ImuConstPtr>,sensor_msgs::PointCloudConstPtr>> measurements;
        std::unique_lock<std::mutex> lk(m_sign);
        con.wait(lk,[&]{
                return (measurements=getMeasurement()).size()!=0;
        });
        lk.unlock();
       m_estimator.lock();
       for (auto &measurement : measurements) {
           auto img_msg = measurement.second;
           double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
           for (auto &imu_msg: measurement.first) {
               double t = imu_msg->header.stamp.toSec();
               double img_t = img_msg->header.stamp.toSec();
               if (t <= img_t) {
                   if (current_time < 0)
                       current_time = t;
                   double dt = t - current_time;
                   assert(dt >= 0);
                   current_time = t;
                   dx = imu_msg->linear_acceleration.x;
                   dy = imu_msg->linear_acceleration.y;
                   dz = imu_msg->linear_acceleration.z;
                   rx = imu_msg->angular_velocity.x;
                   ry = imu_msg->angular_velocity.y;
                   rz = imu_msg->angular_velocity.z;
                   IMU_Preintegration(dt, Eigen::Vector3d(dx, dy, dz), Eigen::Vector3d(rx, ry, rz));
                   //printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz);

               } else {
                   double dt_1 = img_t - current_time;
                   double dt_2 = t - img_t;
                   current_time = img_t;
                   assert(dt_1 >= 0);
                   assert(dt_2 >= 0);
                   assert(dt_1 + dt_2 > 0);
                   double w1 = dt_2 / (dt_1 + dt_2);
                   double w2 = dt_1 / (dt_1 + dt_2);
                   dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
                   dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
                   dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
                   rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
                   ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
                   rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
                   IMU_Preintegration(dt_1, Eigen::Vector3d(dx, dy, dz), Eigen::Vector3d(rx, ry, rz));
                   //printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz);
               }
           }
           map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
           for (unsigned int i = 0; i < img_msg->points.size(); i++)
           {
               int v = img_msg->channels[0].values[i] + 0.5;
               int feature_id = v;
               int camera_id = v;
               double x = img_msg->points[i].x;
               double y = img_msg->points[i].y;
               double z = img_msg->points[i].z;
               double p_u = img_msg->channels[1].values[i];
               double p_v = img_msg->channels[2].values[i];
               double velocity_x = img_msg->channels[3].values[i];
               double velocity_y = img_msg->channels[4].values[i];
               assert(z == 1);
               Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
               xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
               image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
           }
           processImage(image, img_msg->header);
       }
       m_estimator.unlock();
    }
}




vector<pair<vector<sensor_msgs::ImuConstPtr>,sensor_msgs::PointCloudConstPtr>> Estimator::getMeasurement() {

    std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
    while (true)
    {
        if (imu_buf.empty() || fea_buf.empty())
            return measurements;

        if (!(imu_buf.back()->header.stamp.toSec() > fea_buf.front()->header.stamp.toSec()))
        {
            return measurements;
        }

        if (!(imu_buf.front()->header.stamp.toSec() < fea_buf.front()->header.stamp.toSec()))
        {
            cout<<"imu_buf size : "<<imu_buf.size()<<endl;
            cout<<"fea_buf size : "<<fea_buf.size()<<endl;
            fea_buf.pop();
            continue;
        }
        sensor_msgs::PointCloudConstPtr img_msg = fea_buf.front();
        fea_buf.pop();

        std::vector<sensor_msgs::ImuConstPtr> IMUs;
        while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec())
        {
            IMUs.emplace_back(imu_buf.front());
            imu_buf.pop();
        }
        IMUs.emplace_back(imu_buf.front());
        if (IMUs.empty())
            cout<<"no imu between two image"<<endl;
        measurements.emplace_back(IMUs, img_msg);
    }
    return measurements;
}

void Estimator::processFeature(const sensor_msgs::PointCloudConstPtr &fea_msg) {
    m_sign.lock();
    fea_buf.emplace(fea_msg);
    m_sign.unlock();
    con.notify_one();
}

void Estimator::processIMU(const sensor_msgs::ImuConstPtr &imu_msg) {
    m_sign.lock();
    imu_buf.emplace(imu_msg);
    m_sign.unlock();
    con.notify_one();
}

void Estimator::IMU_Preintegration(double dt, const Eigen::Vector3d &linear_acceleration,
                                   const Eigen::Vector3d &angular_velocity) {
    cout<<"Handing IMU...."<<endl;
    if (!first_imu)
    {
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }

    if (!pre_integrations[frame_count])
    {
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }
    if (frame_count != 0)
    {
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        //if(solver_flag != NON_LINEAR)
        tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

        dt_buf[frame_count].push_back(dt);
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

        int j = frame_count;
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
        Vs[j] += dt * un_acc;
    }
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
{

}